#include <stdio.h>
#include <string.h>
#include <errno.h>
#include <iostream>
#include <stdlib.h>     //for using the function sleep
#include <stdio.h>
#include <time.h>
#include <unistd.h>

#include "../source/robot/robot.h"
#include "tests.h"

using namespace std;

bool startDrivingTest (){
	Robot robot;
	robot.servoOpen();
	//robot.rotate(100);
	sleep(3) ;
	//robot.stop();
	robot.servoClose();
	int dist = robot.getDistance();
	cout << endl << "Distance = " << dist << "\n";
	cout << "Ending driving test\n";
	dist = robot.getDistance();
	cout << "Distance (1) = " << dist << "\n";
	dist = robot.getDistance();
	cout << "Distance (2) = " << dist << "\n";
	robot.destroy();
	return 1;
}
